#include <ros/ros.h>
#include <std_srvs/SetBool.h>
#include <sensor_msgs/Joy.h>
#include <ok_hknetcam/HCNetSDK.h>
#include <ok_hknetcam/CapPicture.h>
#include <ok_hknetcam/PtzPreset.h>
namespace
{
  LONG userId;
  //设备信息, 输出参数
  NET_DVR_DEVICEINFO_V40 struDeviceInfoV40 = {0};
  int light_state = 0;
  int wiper_state = 0;
};

bool ptz_preset(ok_hknetcam::PtzPreset::Request &req,
                ok_hknetcam::PtzPreset::Response &res){
  switch(req.set_or_go){
    case 1 :
      res.result = NET_DVR_PTZPreset_Other(::userId, ::struDeviceInfoV40.struDeviceV30.byStartChan, SET_PRESET, req.index);
      ROS_INFO("set preset, num is %d", req.index);
      break;
    case 2 :
      res.result = NET_DVR_PTZPreset_Other(::userId, ::struDeviceInfoV40.struDeviceV30.byStartChan, GOTO_PRESET, req.index);
      ROS_INFO("goto preset, num is %d", req.index);
      sleep(4);
      break;
    case 3 :
      res.result = NET_DVR_PTZPreset_Other(::userId, ::struDeviceInfoV40.struDeviceV30.byStartChan, CLE_PRESET, req.index);
      ROS_INFO("cancle preset, num is %d", req.index);
      break;
    default :
        ROS_INFO("error commond");
        return false;
    }
    return true;
}

bool ir_control(DWORD state){
  if(!NET_DVR_PTZControl_Other(::userId, ::struDeviceInfoV40.struDeviceV30.byStartChan,LIGHT_PWRON, state)){
      printf("pyd1---NET_DVR_LIGHTControl error, %d\n", NET_DVR_GetLastError());
      return false;
  }
  return true;
}
bool camera_capture(std::string filename){
  NET_DVR_JPEGPARA strPicPara = {0};
  strPicPara.wPicQuality = 2;
  char file_tmp[256];
  strPicPara.wPicSize = 0;
  strcpy(file_tmp, filename.c_str());
  if(!NET_DVR_CaptureJPEGPicture(::userId, ::struDeviceInfoV40.struDeviceV30.byStartChan, &strPicPara, file_tmp)){
      printf("pyd1---NET_DVR_CaptureJPEGPicture error, %d\n", NET_DVR_GetLastError());
      return false;
  };
  return true;
}
bool captrue_cb(ok_hknetcam::CapPicture::Request &req, ok_hknetcam::CapPicture::Response &res){
    if(req.picname !=""){
       // string FM;
        //FM = file+req.picname;
        camera_capture(req.picname);
        res.result = req.picname;
    }
    else{
        ROS_INFO("no request");
        return false;
    }
    return true;
}
void ptz_control(DWORD commond, DWORD state){
    if(!NET_DVR_PTZControlWithSpeed_Other(userId, ::struDeviceInfoV40.struDeviceV30.byStartChan,commond, state, 2)){
        printf("pyd1---NET_DVR_PTZControl error, %d\n", NET_DVR_GetLastError());
    }
}
void joy_callback(const sensor_msgs::Joy::ConstPtr& msg){
  if((msg->axes[5]!=-1)&&(msg->axes[2]!=-1)&&(msg->axes[7]==1)){
      ptz_control(TILT_UP, 1);
  }else if((msg->axes[5]!=-1)&&(msg->axes[2]!=-1)&&(msg->axes[7]==-1)){
      ptz_control(TILT_DOWN, 1);
  }else if((msg->axes[5]!=-1)&&(msg->axes[2]!=-1)&&(msg->axes[6]==1)){
      ptz_control(PAN_LEFT, 1);
  }else if((msg->axes[5]!=-1)&&(msg->axes[2]!=-1)&&(msg->axes[6]==-1)){
      ptz_control(PAN_RIGHT, 1);
  }else if(msg->buttons[9]==1){
      ::light_state += 1;
      if(::light_state%2 == 1){
          if(!ir_control(1)){
              ROS_ERROR("ptz turn light on failed");
              ::light_state -= 1;
          }
      }else{
          if(!ir_control(0)){
              ROS_ERROR_STREAM("ptz turn light off failed");
              ::light_state -= 1;
          }
      }
  }else if(msg->buttons[10]==1){
      ::wiper_state += 1;
      if(::wiper_state%2 == 1){
          if(!NET_DVR_PTZControlWithSpeed_Other(userId, ::struDeviceInfoV40.struDeviceV30.byStartChan,WIPER_PWRON, 1, 2)){
              ROS_ERROR("ptz turn wiper on failed");
              ::wiper_state -= 1;
          }
      }else{
          if(!NET_DVR_PTZControlWithSpeed_Other(userId, ::struDeviceInfoV40.struDeviceV30.byStartChan,WIPER_PWRON, 0, 2)){
              ROS_ERROR("ptz turn wiper off failed");
              ::wiper_state -= 1;
          }
      }
  }else if((msg->axes[2]==-1)&&(msg->axes[7]==1)){
    ptz_control(ZOOM_IN, 1);
  }else if((msg->axes[2]==-1)&&(msg->axes[7]==-1)){
    ptz_control(ZOOM_OUT, 1);
  }else if((msg->axes[2]==-1)&&(msg->axes[7]==0)){
    ptz_control(ZOOM_OUT, 0);
  }else if((msg->axes[2]==-1)&&(msg->axes[6]==1)){
    ptz_control(FOCUS_NEAR, 1);
  }else if((msg->axes[2]==-1)&&(msg->axes[6]==-1)){
    ptz_control(FOCUS_FAR, 1);
  }else if((msg->axes[2]==-1)&&(msg->axes[6]==0)){
    ptz_control(FOCUS_FAR, 0);
  }else if((msg->axes[5]==-1)&&(msg->axes[6]==1)){//光圈+
    ptz_control(IRIS_OPEN, 1);
  }else if((msg->axes[5]==-1)&&(msg->axes[6]==-1)){//光圈-
    ptz_control(IRIS_CLOSE, 1);
  }else if((msg->axes[5]==-1)&&(msg->axes[6]==0)){//光圈-
    ptz_control(IRIS_CLOSE, 0);
  }
}
int main(int argc, char** argv){
  ros::init(argc, argv, "ok_hknetcam");
   //---------------------------------------
  // 初始化
  // std::string hcnetsdk_path = "/home/zq/work/reinovo/baboon_ws/src/ok_hknetcam/libs";
  // NET_DVR_LOCAL_SDK_PATH sdk_path;
  // memcpy(sdk_path.sPath, hcnetsdk_path.c_str(), hcnetsdk_path.size());
  // NET_DVR_SetSDKInitCfg(NET_SDK_INIT_CFG_TYPE::NET_SDK_INIT_CFG_SDK_PATH, (void *)&sdk_path);
  // NET_DVR_SetSDKInitCfg(NET_SDK_INIT_CFG_TYPE::NET_SDK_INIT_CFG_LIBEAY_PATH, (void *)&sdk_path);
  // NET_DVR_SetSDKInitCfg(NET_SDK_INIT_CFG_TYPE::NET_SDK_INIT_CFG_SSLEAY_PATH, (void *)&sdk_path);
  NET_DVR_Init();
  //设置连接时间与重连时间
  NET_DVR_SetConnectTime(2000, 1);
  NET_DVR_SetReconnect(10000, true);

  //---------------------------------------
  // 注册设备

  //登录参数，包括设备地址、登录用户、密码等
  NET_DVR_USER_LOGIN_INFO struLoginInfo = {0};
  struLoginInfo.bUseAsynLogin = 0; //同步登录方式
  strcpy(struLoginInfo.sDeviceAddress, "192.168.1.22"); //设备IP地址
  struLoginInfo.wPort = 8000; //设备服务端口
  strcpy(struLoginInfo.sUserName, "baboon"); //设备登录用户名
  strcpy(struLoginInfo.sPassword, "reinovo888"); //设备登录密码

  ::userId = NET_DVR_Login_V40(&struLoginInfo, &::struDeviceInfoV40);
  if (::userId < 0)
  {
      printf("Login failed, error code: %d\n", NET_DVR_GetLastError());
      NET_DVR_Cleanup();
      return -1;
  }
  ros::NodeHandle nh;
  ros::ServiceServer cam_service = nh.advertiseService("capture_picture", captrue_cb);
  ros::ServiceServer ptz_service = nh.advertiseService("ptz_preset", ptz_preset);
  ros::Subscriber sub = nh.subscribe("joy", 1, joy_callback);
  ros::spin();
  return 0;
}